#pragma once

#include "BaseEntity.h"
#include <SFML/System.hpp>
#include <SFML/Graphics.hpp>
#include <iostream>
#include <math.h>

struct CollisionData
{
	float Penetration;
	sf::Vector2f Normal;
	sf::Vector2f CollisionPoint;
	sf::Vector2f RelativeVelocity;
	int CollisionCount;
};

#define DEBUG_PHYSICS 0

class PhysicsObject : public BaseEntity
{
private:
	float Mass;
	sf::Vector2f CentreG;
	sf::Vector2f Resultant;
	sf::Vector2f ResultantPos;
	sf::Vector2f Point;

	bool Moveable; //Never moveable

	bool Solid;
	float MOI;

	float mRadius; //Circles only

	bool bIsCircle;
	bool bIsPolygon;

	float Bounce;

#if DEBUG_PHYSICS == 1
	sf::Shape shpAABB;
	sf::Shape shpCollPoint;
	sf::Shape shpNormal;

	sf::Shape sL1;
	sf::Shape sL2;
	sf::Shape sL3;
	sf::Shape sL4;

	sf::Shape sP1;
	sf::Shape sP2;
	sf::Shape sP3;
	sf::Shape sP4;

	sf::Shape sN1;
	sf::Shape sN2;
	sf::Shape sN3;
	sf::Shape sN4;
#endif

	int PointCount;

	float AngularAcceleration;
	sf::Vector2f Acceleration;
	sf::Vector2f Velocity;
	float AngularVelocity;

	float LinearDamping;
	float AngularDamping;

public:

	sf::Vector2f FarthestPoint(sf::Vector2f dir);

	bool IsPolygon() {return bIsPolygon;};
	bool IsCircle() {return bIsCircle;};

	void SetMoveable(bool m) {Moveable = m;};
	bool GetMoveable() {return Moveable;};

	void SetSolid(bool s) {Solid = s;};
	bool GetSolid() {return Solid;};

	void SetBounce(float b) {Bounce = b;};
	float GetBounce() {return Bounce;};

	void SetLinearDamping(float d) {LinearDamping = d;};
	float GetLinearDamping() {return LinearDamping;};

	void SetAngularDamping(float d) {AngularDamping = d;};
	float GetAngularDamping() {return LinearDamping;};

	std::vector<sf::Vector2f> Coords;

	sf::FloatRect AABB;

	int GetPointCount() {return PointCount;};
	void AddPoint(sf::Vector2f Point);

	void OnCollide(PhysicsObject* ent1, PhysicsObject* ent2, CollisionData CollideInfo);

	bool AABBCollision(sf::FloatRect One, sf::FloatRect Two);
	PhysicsObject(sf::RenderWindow *App);
	~PhysicsObject(void);
	void Think();
	void Draw();
	void Register();

	void Integrate(float delta);

	void PhysicsInitPolygon(BaseEntity* Caller, sf::Vector2f *Points, int PointCount);
	void PhysicsInitCircle(BaseEntity* Caller, float radius);

	void Activate(BaseEntity* Creator);

	void SetMass(float mass) {Mass = mass; UpdateMOI();};
	float GetMass() {return Mass;};

	void UpdateMOI();

	void ApplyForceOffset(sf::Vector2f Force, sf::Vector2f Offset);

	sf::Vector2f GetPointVelocity(sf::Vector2f GlobalPoint);

	sf::FloatRect GetAABB();

	void SetVelocity(sf::Vector2f Vel) { Velocity = Vel;};
	void SetVelocity(float x, float y) { Velocity.x = x; Velocity.y = y;};
	void AddVelocity(sf::Vector2f Vel) { Velocity += Vel;};
	sf::Vector2f GetVelocity() {return Velocity;};

	void SetAngularVel(float angvel) {AngularVelocity = angvel;};
	float GetAngularVel() {return AngularVelocity;};
};